model;

for i = 1:n
    qi = Q(i, :);
    T_th = fkine(DH_para, qi);
    T_real = T_b0 * T_th * T_tool;
    x_th(3*i-2: 3*i, :) = T_th(1:3, 4);
    x_real(3*i-2: 3*i, :) = T_real(1:3, 4);
end

alpha = 0.01;
beta = 0.9;

iter_max = 100;
para_k = zeros(9, 1);
para_k(4) = 0;
finish = false;

for k = 1:iter_max
    
    for i = 1:n
        qi = Q(i, :);
        Jacobian(3*i-2:3*i, :) = calculate_Jacobian(DH_para, para_k, qi);
    end
    
    delta_x = calculate_error(DH_para, para_k, x_real, Q);
    delta_para = pinv(Jacobian) * delta_x;
    
    tk = 1;
    while norm(calculate_error(DH_para, para_k + tk * delta_para, x_real, Q)) > norm(delta_x)
        tk = beta * tk;
        if tk < 0.1
            finish = true;
            delta_para = 0 * delta_para;
            break;
        end
    end
    
    para_k = para_k + tk * delta_para;
    if finish
        break;
    end
end

disp(para_k)